
#include "taiic_v4l2.h"
#include "taiic_v4l2_config.h"
#include <thread>
#include "dirent.h"
#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include <assert.h>
#include <semaphore.h>

using namespace std;

sem_t semA, semB;

int readLen_left = 0;
int readLen_right = 0;
////////////////////////////////////ringBuffere class object////////////////////////////////////
RingBuf_Taiic buf_left, buf_right;
#define CAM_MAX_SIZE 384000 * 3840
#define  WRITE_LEN 384000
////////////////////////////////////////////////////////////////////////////////////
void *camFunc(void *ptr);
char readBuff_left[WRITE_LEN+1]={0};
char readBuff_right[WRITE_LEN+1]={0};

long dataLen=0;

int countPicR = 0;
int countPicL = 0;

bool ret1 = mkdir("/data/biao", 0755);
const char *out_left_file = "/data/biao/outputleft.yuv";
const char *out_right_file = "/data/biao/outputright.yuv";
std::ofstream outstream_left(out_left_file, std::ios::out | std::ios::binary);
std::ofstream outstream_right(out_right_file, std::ios::out | std::ios::binary);

std::ofstream ofstampleft("/data/biao/timestampleft.txt", std::ios::trunc);
std::ofstream ofstampright("/data/biao/timestampright.txt", std::ios::trunc);

std::ofstream ofstampleft_dif("/data/biao/left_ts_diff.txt", std::ios::trunc);
std::ofstream ofstampright_dif("/data/biao/right_ts_diff.txt", std::ios::trunc);

long last_ts_left = 0;
long last_ts_right = 0;

char * cType=NULL;

void TaiicV4l2::getFrame()
{
    std::memset(&rightTs, 0, sizeof(rightTs));
    std::memset(&leftTs, 0, sizeof(leftTs));

    pthread_t tid;
    std::thread getFrameData(&TaiicV4l2::threadFunction, this);
    pthread_create(&tid, NULL, camFunc, NULL);

    getFrameData.join();
    pthread_join(tid, NULL);
}

void vi_frame_data_cback(char *camtype, void *data, long length, long timestamp)
{
    sem_wait(&semA);
    dataLen = length;
    cType = camtype;
    //printf("cType=%s, dataLen=%ld\n",cType,dataLen);
    if (strcmp(camtype, "left") == 0)
    {
        // std::cout << countPicL << " countPic timestamp: "<< timestamp <<"  "<< camtype << std::endl;
        //outstream_left.write((char *)data, length);
       
        if (buf_left.getRingbufferValidLen() < CAM_MAX_SIZE)
        {
            buf_left.writeRingbuffer((char *)data, length); 
        }
    
        if(ofstampleft.is_open())
        {
            ofstampleft << countPicL << " countPic_timestamp: "<< timestamp <<"  "<< camtype << std::endl;
        }

        countPicL++;
        if ((countPicL > 1) && ofstampleft_dif.is_open())
        {
            std::cout << countPicL << " : " << (timestamp / 1000000) - last_ts_left << camtype <<std::endl; // std::cout << countPicL << " timestamp diff: "<< (timestamp/1000000)-last_ts_left << "  "<< camtype << std::endl;
            ofstampleft_dif << (timestamp/1000000)-last_ts_left << std::endl;
        }
        last_ts_left = timestamp / 1000000; 
    }
    else if (strcmp(camtype, "right") == 0)
    {
        // std::cout << countPicR << " countPic timestamp: "<< timestamp <<"  "<< camtype << std::endl;
       // outstream_right.write((char *)data, length);
    
         if (buf_right.getRingbufferValidLen()<CAM_MAX_SIZE)
         {
            buf_right.writeRingbuffer((char *)data, length);
         }

        if(ofstampright.is_open())
        {
            ofstampright << countPicR << " countPic_timestamp: "<< timestamp <<"  "<< camtype << std::endl;
        }
        
        countPicR++;
        if ((countPicR > 1)&&(ofstampright_dif.is_open()))
        {
            std::cout << countPicR << " : " << (timestamp / 1000000) - last_ts_right << camtype<<std::endl; // std::cout << countPicL << " timestamp diff: "<< (timestamp/1000000)-last_ts_left << "  "<< camtype << std::endl;
            ofstampright_dif << (timestamp/1000000)-last_ts_right << std::endl;
        }
        last_ts_right = timestamp / 1000000;
    }
    sem_post(&semB);//mtx.unlock(); 
}


int main(int argc, char *argv[])
{
    buf_left.buffer_size = CAM_MAX_SIZE;
    buf_right.buffer_size = CAM_MAX_SIZE;
    buf_left.initRingbuffer();
    buf_right.initRingbuffer();

    // 初始化信号量
    sem_init(&semA, 0, 1);
    sem_init(&semB, 0, 0);

    TaiicV4l2 s_taiic_v4l2;

    s_taiic_v4l2.initDevice();

    s_taiic_v4l2.startCapt(vi_frame_data_cback);
    std::printf("==open success==\n");

__EXIT:
    s_taiic_v4l2.stopCapt();
    std::printf("==stop success==\n");
    s_taiic_v4l2.closeDevice();

    
    buf_left.releaseRingbuffer();
    buf_right.releaseRingbuffer();
     outstream_left.close();
    outstream_right.close();
    ofstampleft.close();
    ofstampright.close();
    ofstampleft_dif.close();
    ofstampright_dif.close();

    return 0;
}

void *camFunc(void *ptr)
{
    memset(readBuff_left,0,WRITE_LEN+1);
    memset(readBuff_right,0,WRITE_LEN+1);
    while (1)
    {
        sem_wait(&semB);
        if(strcmp(cType, "left") == 0)
        {
            if (buf_left.getRingbufferValidLen() > dataLen)
            {
                readLen_left = buf_left.readRingbuffer((char*)readBuff_left, dataLen);
                std::printf("readLen_left=%d\n", readLen_left);
                outstream_left.write((char*)readBuff_left, dataLen);
            }
        }
        else if(strcmp(cType, "right") == 0)
        {
            if (buf_right.getRingbufferValidLen() > dataLen)
            {
                readLen_right = buf_right.readRingbuffer((char*)readBuff_right, dataLen);
                std::printf("readLen_right=%d\n", readLen_right);
                outstream_right.write((char*)readBuff_right, dataLen);
            }
        }
        sem_post(&semA);    
    }
    printf("%s quit..\n", __func__);
    return NULL;
}